﻿#include <osgEarth/MapNode>
#include <osgEarth/GeoMath>
#include <osgEarth/Terrain>
#define SRSWGS84 osgEarth::SpatialReference::get("wgs84")

bool RadianLLH2XYZ(osgEarth::MapNode* mapNode, const osg::Vec3d& vecLLH, osg::Vec3d& vecXYZ )
{
	if(mapNode)
	{
		mapNode->getMapSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
			vecLLH.y(), vecLLH.x(), vecLLH.z(), vecXYZ.x(), vecXYZ.y(), vecXYZ.z());
		return true;
	}

	return false;
}

bool DegreeLLH2XYZ(osgEarth::MapNode* mapNode, const osg::Vec3d& vecLLH, osg::Vec3d& vecXYZ )
{
	if(mapNode)
	{
		osg::Vec3d vecRadianLLH(osg::DegreesToRadians(vecLLH.x()), osg::DegreesToRadians(vecLLH.y()), vecLLH.z());
		return RadianLLH2XYZ(mapNode, vecRadianLLH, vecXYZ);
	}

	return false;
}

bool XYZ2RadianLLH(osgEarth::MapNode* mapNode, const osg::Vec3d& vecXYZ, osg::Vec3d& vecLLH )
{
	if(mapNode)
	{
		mapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
			vecXYZ.x(), vecXYZ.y(), vecXYZ.z(), vecLLH.y(), vecLLH.x(), vecLLH.z());
		return true;
	}

	return false;
}

bool XYZ2DegreeLLH(osgEarth::MapNode* mapNode, const osg::Vec3d& vecXYZ, osg::Vec3d& vecLLH )
{
	if(mapNode)
	{
		mapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
			vecXYZ.x(), vecXYZ.y(), vecXYZ.z(), vecLLH.y(), vecLLH.x(), vecLLH.z());
		vecLLH.x() = osg::RadiansToDegrees(vecLLH.x());
		vecLLH.y() = osg::RadiansToDegrees(vecLLH.y());
		return true;
	}

	return false;
}

bool ScreenXY2RadiaLLH(osgEarth::MapNode* mapNode, osg::View* view, float fX, float fY, osg::Vec3d& vecLLH )
{
	if(mapNode && view)
	{
		osg::Vec3d vecWorld;
		if(mapNode->getTerrain()->getWorldCoordsUnderMouse(view, fX, fY, vecWorld))
		{
			mapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
				vecWorld.x(), vecWorld.y(), vecWorld.z(), vecLLH.y(), vecLLH.x(), vecLLH.z());

			return true;
		}
	}

	return false;
}

bool ScreenXY2DegreeLLH(osgEarth::MapNode* mapNode, osg::View* view, float fX, float fY, osg::Vec3d& vecLLH )
{
	if(ScreenXY2RadiaLLH(mapNode, view, fX, fY, vecLLH))
	{
		vecLLH.x() = osg::RadiansToDegrees(vecLLH.x());
		vecLLH.y() = osg::RadiansToDegrees(vecLLH.y());
		return true;
	}

	return false;
}

bool ScreenXY2DegreeLLH(osgEarth::MapNode* mapNode, osg::View* view, float fX, float fY, double& dLon, double& dLat, double& dHei)
{
	osg::Vec3d vecLLH;
	if(ScreenXY2RadiaLLH(mapNode, view, fX, fY, vecLLH))
	{
		dLon = osg::RadiansToDegrees(vecLLH.x());
		dLat = osg::RadiansToDegrees(vecLLH.y());
		return true;
	}

	return false;
}

bool ScreenXY2RadiaLLH(osgEarth::MapNode* mapNode, osg::View* view, float fX, float fY, double& dLon, double& dLat, double& dHei)
{
	if(mapNode && view)
	{
		osg::Vec3d vecWorld;
		if(mapNode->getTerrain()->getWorldCoordsUnderMouse(view, fX, fY, vecWorld))
		{
			mapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
				vecWorld.x(), vecWorld.y(), vecWorld.z(), dLat, dLon, dHei);
			return true;
		}
	}

	return false;
}

bool RadiaLLH2Matrix(osgEarth::MapNode* mapNode, const osg::Vec3d& vecLLH,osg::Matrix& matrix )
{
	if(mapNode)
	{
		mapNode->getMapSRS()->getEllipsoid()->
			computeLocalToWorldTransformFromLatLongHeight(vecLLH.y(),vecLLH.x(),vecLLH.z(),matrix);

		return true;
	}

	return false;
}

bool DegreeLLH2Matrix(osgEarth::MapNode* mapNode, const osg::Vec3d& vecLLH,osg::Matrix& matrix )
{
	return RadiaLLH2Matrix(mapNode, osg::Vec3d(osg::DegreesToRadians(vecLLH.x()),osg::DegreesToRadians(vecLLH.y()),vecLLH.z()), matrix);
}

bool XYZ2Matrix(osgEarth::MapNode* mapNode, const osg::Vec3d& vecXYZ,osg::Matrix& matrix )
{
	osg::Vec3d vecLLHRadia;
	if(XYZ2RadianLLH(mapNode, vecXYZ, vecLLHRadia))
	{
		return RadiaLLH2Matrix(mapNode, vecLLHRadia, matrix);
	}

	return false;
}

bool DegreeLL2LLH(osgEarth::MapNode* mapNode, osg::Vec3d& vecLLA)
{
	if(mapNode)
	{
		double dHeight = 0.0;
		if(mapNode->getTerrain()->getHeight(mapNode->getMapSRS(), vecLLA.x(),vecLLA.y(), &dHeight))
		{
			vecLLA.z() = dHeight;
			return true;
		}

		return true;
	}

	return false;
}

bool RadiaLL2LLH(osgEarth::MapNode* mapNode, osg::Vec3d& vecLLA)
{
	osg::Vec3d vecLLAD(osg::RadiansToDegrees(vecLLA.x()),osg::RadiansToDegrees(vecLLA.y()),vecLLA.z());
	if(DegreeLL2LLH(mapNode, vecLLA))
	{
		vecLLA.z() = vecLLAD.z();
		return true;
	}

	return false;
}

bool ConvertLocalWorldCoordToScreen(osgEarth::MapNode* mapNode, osg::View* view, const osg::Vec3d& pos, osg::Vec2d& screenPos )
{
	if(mapNode && view)
	{
		osg::Camera* cam = view->getCamera();
		osg::MatrixList worldMatrixList = cam->getWorldMatrices(mapNode);
		osg::Matrix worldMatrix = worldMatrixList.at(0);
		osg::Matrix viewMatrix = cam->getViewMatrix();
		osg::Matrix projMatrix = cam->getProjectionMatrix();
		//		osg::Matrix camMatrix = cam->getViewport()->computeWindowMatrix();
		osg::Vec4d in = osg::Vec4d(pos.x(), pos.y(), pos.z(), 1.0);
		osg::Vec4d out = in * worldMatrix ;
		out =out * viewMatrix;
		out =out  * projMatrix;
		//	out =out  * camMatrix;

		if (out.w() <= 0.0) return false;  //如果out.w()小于0说明在背面不被拣选

		out.x() /= out.w();
		out.y() /= out.w();
		out.z() /= out.w();

		out.x() = out.x() * 0.5 + 0.5;
		out.y() = out.y() * 0.5 + 0.5;
		out.z() = out.z() * 0.5 + 0.5;

		if(!cam->getViewport())
		{
			return false;
		}
		int nScreenW = cam->getViewport()->width();
		int nScreenH = cam->getViewport()->height();

		screenPos.x() = out.x() * nScreenW;
		screenPos.y() = out.y() * nScreenH;   

		return true;
	}
	return false;
}

double GetGeoDistance(osgEarth::MapNode* mapNode, double dSLon, double dSLat, double dELon, double dELat )
{
	if(mapNode)
	{
		return osgEarth::GeoMath::distance(dSLat,dSLon, dELat, dELon, mapNode->getMapSRS()->getEllipsoid()->getRadiusEquator());
	}

	return  0.0; 
}

double GetGeoDistanceDegree(osgEarth::MapNode* mapNode, double dSLon, double dSLat, double dELon, double dELat )
{
	if(mapNode)
	{
		return osgEarth::GeoMath::distance(osg::DegreesToRadians(dSLat),osg::DegreesToRadians(dSLon), osg::DegreesToRadians(dELat), osg::DegreesToRadians(dELon), mapNode->getMapSRS()->getEllipsoid()->getRadiusEquator());
	}

	return  0.0; 
}

double GetGeoRhumbDistanceDegree(osgEarth::MapNode* mapNode,double dSLon, double dSLat, double dELon, double dELat)
{
	if(mapNode)
	{
		return osgEarth::GeoMath::rhumbDistance(osg::DegreesToRadians(dSLat),osg::DegreesToRadians(dSLon), osg::DegreesToRadians(dELat), osg::DegreesToRadians(dELon), mapNode->getMapSRS()->getEllipsoid()->getRadiusEquator());
	}

	return  0.0; 
}

osg::Vec3d GeoMidPointDegree(osg::Vec3d sLLH, osg::Vec3d eLLH)
{
	osg::Vec3d llh;

	osgEarth::GeoMath::midpoint(osg::DegreesToRadians(sLLH.y()),osg::DegreesToRadians(sLLH.x()),
		osg::DegreesToRadians(eLLH.y()),osg::DegreesToRadians(eLLH.x()),
		llh.y(),llh.x());
	llh.z() = (sLLH.z() + eLLH.z()) * 0.5; 

	return llh;
}

double RhumbBearing(osg::Vec3d sLLH, osg::Vec3d eLLH)
{
	return osgEarth::GeoMath::rhumbBearing(osg::DegreesToRadians(sLLH.y()),osg::DegreesToRadians(sLLH.x()),
		osg::DegreesToRadians(eLLH.y()),osg::DegreesToRadians(eLLH.x()));
}

double ComputeJD0h()
{
	osgEarth::DateTime dUtc = osgEarth::DateTime();
	//计算儒略日
	int y, m, B;
	y = dUtc.year(); 
	m = dUtc.month(); 
	//如果日期在1月或2月，则被看作是在前一年的13月或14月
	if (dUtc.month() <= 2) 
	{ 
		y = dUtc.year() - 1; 
		m = dUtc.month() + 12; 
	}
	/* 对格里高利历(即1582年10月15日以后)，有
	B = 2 - Y/100 + Y/400.
	另外，对于儒略历(即1582年10月15日之前)，取B=0. */
	B = -2; 
	if (dUtc.year() > 1582 || (dUtc.year() == 1582 && (dUtc.month() > 10 || (dUtc 
		.month() == 10 && dUtc.day() >= 15)))) 
	{ 
		B = y / 400 - y / 100; 
	}
	return (floor(365.25 * y) + 
		floor(30.6001 * (m + 1)) + B + 1720996.5 + 
		dUtc.day()); 
}

double ComputeJD()
{
	osgEarth::DateTime dUtc = osgEarth::DateTime();
	//计算儒略日
	int y, m, B;
	y = dUtc.year(); 
	m = dUtc.month(); 
	//如果日期在1月或2月，则被看作是在前一年的13月或14月
	if (dUtc.month() <= 2) 
	{ 
		y = dUtc.year() - 1; 
		m = dUtc.month() + 12; 
	}
	// 对格里高利历(即1582年10月15日以后)，有
	//	B = 2 - Y/100 + Y/400.
	//	另外，对于儒略历(即1582年10月15日之前)，取B=0. 
	B = -2; 
	if (dUtc.year() > 1582 || (dUtc.year() == 1582 && (dUtc.month() > 10 || (dUtc 
		.month() == 10 && dUtc.day() >= 15)))) 
	{ 
		B = y / 400 - y / 100; 
	}
	return (floor(365.25 * y) + 
		floor(30.6001 * (m + 1)) + B + 1720996.5 + 
		dUtc.day() + dUtc.hours() / 24.0 ); 
}

double UT12GMST()
{
	double dT0 = (ComputeJD0h() - 2451545) /36525;
	double dT = (ComputeJD() - 2451545) / 36525;

	osgEarth::DateTime dUtc = osgEarth::DateTime();
	double dUT1 = dUtc.hours()* 3600;

	double dGMST = 24110.54841 + 8640184.812866 * dT0 + 1.002737909350795 * dUT1 +
		0.093104 * dT * dT - 0.0000062 * dT * dT * dT;

	return dGMST;
}

bool J2000XYZ2WGS84XYZ(osgEarth::MapNode* mapNode, osg::View* view, float fX, float fY, double& dX, double& dY, double& dZ )
{
	if (mapNode)
	{
		osg::Vec3d vecWorld;
		if(mapNode->getTerrain()->getWorldCoordsUnderMouse(view, fX, fY, vecWorld))
		{
			double dAngle = UT12GMST() / (24*3600) * 2*(osg::PI);
			dX = osg::Vec3d(cos(dAngle), sin(dAngle), 0) * vecWorld;
			dY = osg::Vec3d(-sin(dAngle), cos(dAngle), 0) * vecWorld;
			dZ = osg::Vec3d(0, 0 ,1) * vecWorld;

			return true;
		}
	}
	return false;
}
